// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SLAM2DParticle.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Localization
{
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// State contained by a particle used in a SLAM2D particle filter.
    /// </summary>
    public struct SLAM2DParticle
    {
        /// <summary>
        /// Initializes a new instance of the SLAM2DParticle struct.
        /// </summary>
        /// <param name="map">The map object to initialize with</param>
        /// <param name="pose">The pose of the particle on the map</param>
        public SLAM2DParticle(GlobalMap map, Pose2D pose)
            : this()
        {
            this.Map = map;
            this.Pose = pose;
        }

        /// <summary>
        /// Gets the pose of the robot, in global map coordinates.
        /// </summary>
        public Pose2D Pose { get; internal set; }

        /// <summary>
        /// Gets the global map tracked by this particle.
        /// </summary>
        public GlobalMap Map { get; private set; }

        /// <summary>
        /// Gets the surface extracted from the map in the last iteration.
        /// </summary>
        public IHorizontalDepthProfile SurfaceFromMap { get; internal set; }

        /// <summary>
        /// Gets the number of times we updated this particle (between re-samplings)
        /// </summary>
        public int UpdateCount { get; internal set; }

        /// <summary>
        /// Gets the current particle weight.
        /// </summary>
        public double Weight { get; internal set; }

        /// <summary>
        /// Gets the last observation match score
        /// </summary>
        public double SingleIterationWeight { get; internal set; }

        /// <summary>
        /// Gets the last drive feedback state.
        /// </summary>
        public IDriveState DriveFeedbackState { get; internal set; }

        /// <summary>
        /// Creates a derived particle from the current one. 
        /// </summary>
        /// <param name="createWritableMap">If true, the particle acquires a writable copy of the parent's map</param>
        /// <returns>A particle derived from this one</returns>
        internal SLAM2DParticle Clone(bool createWritableMap)
        {
            SLAM2DParticle newParticle = new SLAM2DParticle(createWritableMap ? this.Map.MakeWritableCopy() : this.Map, this.Pose);
            newParticle.SurfaceFromMap = this.SurfaceFromMap;
            newParticle.SingleIterationWeight = this.SingleIterationWeight;
            newParticle.Weight = this.Weight;
            newParticle.DriveFeedbackState = this.DriveFeedbackState;
            return newParticle;
        }
    }
}
